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ABSTRACT 

Aircraft  navigation  information  (position,  velocity,  and  at¬ 
titude)  can  be  determined  using  optical  measurements  from 
imaging  sensors  combined  with  an  inertial  navigation  sys¬ 
tem.  This  can  be  accomplished  by  tracking  the  locations  of 
optical  features  in  multiple  images  and  using  the  resulting 
geometry  to  estimate  and  remove  inertial  errors. 

A  critical  factor  governing  the  performance  of  image-aided 
inertial  navigation  systems  is  the  robustness  of  the  fea¬ 
ture  tracking  algorithm.  Previous  research  has  shown  the 
strength  of  rigorously  coupling  the  image  and  inertial  sen¬ 
sors  at  the  measurement  level  using  a  tactical-grade  inertial 
sensor.  While  the  tactical-grade  inertial  sensor  is  a  reason¬ 
able  choice  for  larger  platforms,  the  greater  physical  size 
and  cost  of  the  sensor  limits  its  use  in  smaller,  low-cost 
platforms. 

In  this  paper,  an  image-aided  inertial  navigation  algorithm 
is  implemented  using  a  multi-dimensional  stochastic  fea¬ 
ture  tracker.  In  contrast  to  previous  research,  the  algo¬ 
rithms  are  specifically  evaluated  for  operation  using  low- 
cost,  CMOS  imagers  and  MEMS  inertial  sensors.  The  per¬ 
formance  of  the  resulting  image-aided  inertial  navigation 
system  is  evaluated  using  Monte  Carlo  simulation  and  ex¬ 


perimental  data  and  compared  to  the  performance  using 
more  expensive  inertial  sensors. 

INTRODUCTION 

Motivation 

As  mentioned  in  our  previous  research  [18],  [24],  [22], 
the  benefits  of  tightly  integrating  navigation  sensors,  such 
as  inertial  measurement  units  (IMU)  and  global  position¬ 
ing  system  (GPS)  receivers,  is  well-known.  The  compli¬ 
mentary  characteristics  of  the  two  sensors  allow  the  inte¬ 
grated  system  to  perform  at  levels  which  are  difficult  to 
attain  with  either  sensor  alone  (see  [3]).  As  a  result,  in¬ 
tegrated  IMU/GPS  systems  have  become  common,  espe¬ 
cially  in  military-grade  navigation  systems.  Unfortunately, 
GPS  signals  are  not  available  in  all  locations,  which  moti¬ 
vates  the  development  of  a  non-GPS  based  navigation  ref¬ 
erence  which  can  aid  an  inertial  navigation  system. 

One  non-GPS  navigation  approach  is  to  integrate  a  cam¬ 
era  with  an  inertial  sensor  [19],  [17],  [2],  This  technique 
has  some  important  advantages.  Foremost,  the  sensors  can 
operate  in  environments  where  GPS  is  difficult  to  receive 
(e.g.,  indoors,  under  trees,  underwater,  etc.).  Secondly,  the 
sensors  are  completely  passive  and  do  not  require  the  trans¬ 
mission  (or  reception)  of  radio  signals.  As  a  result,  optical 
and  inertial  sensors  are  immune  to  disruptions  in  the  radio 
spectrum. 

Previous  work  has  shown  the  value  of  fusing  images  and 
inertial  measurements  for  navigation  [23].  In  this  paper,  an 
image-aided  inertial  navigation  algorithm  is  implemented 
using  a  multi-dimensional  stochastic  feature  tracker.  In 
contrast  to  previous  research,  the  algorithms  are  specifi¬ 
cally  evaluated  for  operation  on  low-cost,  complementary 
metal-oxide  semiconductor  (CMOS)  imagers  and  micro- 
electro-mechanical  systems  (MEMS)  consumer-grade  iner¬ 
tial  sensors.  The  performance  of  the  resulting  image  aided 
inertial  navigation  system  is  evaluated  using  Monte  Carlo 
simulation  and  experimental  data  and  compared  to  the  per¬ 
formance  using  tactical-grade  inertial  sensors.  This  effort 
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is  part  of  ongoing  research  into  fusion  of  imaging  and  iner¬ 
tial  sensors  for  long-term  autonomous  navigation. 

Current  Methods 

It  is  well-known  that  optical  measurements  provide  ex¬ 
cellent  navigation  information,  when  interpreted  properly. 
Optical  navigation  is  not  new.  Pilotage  is  the  oldest  and 
most  natively  familiar  form  of  navigation  to  humans  and 
other  animals.  For  centuries,  navigators  have  utilized  me¬ 
chanical  instruments  such  as  astrolabes,  sextants,  and  drift- 
meters  [14]  to  make  precision  observations  of  the  sky  and 
ground  in  order  to  determine  their  position,  velocity,  and 
attitude. 

The  difficulty  in  using  optical  measurements  for  au¬ 
tonomous  navigation  (that  is,  without  human  intervention) 
has  always  been  in  the  interpretation  of  the  image,  a  diffi¬ 
culty  shared  with  Automatic  Target  Recognition  (ATR).  In¬ 
deed,  when  celestial  observations  are  used,  the  ATR  prob¬ 
lem  in  this  structured  environment  is  tractable,  and  auto¬ 
matic  star  trackers  are  widely  used  for  space  navigation  and 
ICBM  guidance  [6,  15,  16].  When  ground  images  are  to 
be  used,  the  difficulties  associated  with  image  interpreta¬ 
tion  are  paramount.  At  the  same  time,  the  problems  asso¬ 
ciated  with  the  use  of  optical  measurements  for  navigation 
are  somewhat  simpler  than  ATR.  Moreover,  recent  develop¬ 
ments  in  feature  tracking  algorithms,  miniaturization,  and 
reduction  in  cost  of  inertial  sensors  and  optical  imagers, 
aided  by  the  continuing  improvement  in  microprocessor 
technology,  motivates  the  use  of  inertial  measurements  to 
aid  the  task  of  feature  tracking  in  image  sequences. 

Image-aiding  methods  are  typically  classified  as  either 
feature-based  or  optic  flow-based,  depending  on  how  the 
image  correspondence  problem  is  addressed.  Feature- 
based  methods  determine  correspondence  for  “landmarks” 
in  the  scene  over  multiple  frames,  while  optic  flow-based 
methods  typically  determine  correspondence  for  a  whole 
portion  of  the  image  between  frames.  A  good  reference 
on  image  correspondence  is  [8],  Optic  flow  methods  have 
been  proposed  generally  for  elementary  motion  detection, 
focusing  on  determining  relative  velocity,  angular  rates,  or 
obstacle  avoidance  [5], 

Feature  tracking-based  navigation  methods  have  been  pro¬ 
posed  both  for  fixed-mount  imaging  sensors  or  gimbal 
mounted  detectors  which  “stare”  at  the  target  of  interest, 
in  a  manner  similar  to  the  gimballed  infrared  seeker  on 
heat-seeking,  air-to-air  missiles.  Many  feature  tracking- 
based  navigation  methods  exploit  knowledge  (either  a  pri¬ 
ori ,  through  binocular  stereopsis,  or  by  exploiting  terrain 
homography)  of  the  target  location  and  solve  the  inverse 
trajectory  projection  problem  [1, 12].  If  no  a  priori  knowl¬ 
edge  of  the  scene  is  provided,  egomotion  estimation  is 
completely  correlated  with  estimating  the  scene.  This  is  re¬ 


ferred  as  the  structure  from  motion  (SFM)  problem.  A  the¬ 
oretical  development  of  the  geometry  of  fixed-target  track¬ 
ing,  with  no  a  priori  knowledge  is  provided  in  [13],  An  on¬ 
line  (Extended  Kalman  Filter-based)  method  for  calculat¬ 
ing  a  trajectory  by  tracking  features  at  an  unknown  location 
on  the  Earth’s  surface,  provided  the  topography  is  known  is 
given  in  [4],  Finally,  navigation-grade  inertial  sensors  and 
terrain  images  collected  on  a  T-38  “Talon”  were  processed 
and  the  potential  benefits  of  optical-aided  inertial  sensors 
are  experimentally  demonstrated  in  [17], 

A  rigorous,  stochastic  projection  algorithm  is  presented 
in  [24],  which  incorporates  inertial  measurements  into  a 
predictive  feature  transformation,  effectively  constraining 
the  resulting  correspondence  search  space.  The  algorithm 
was  incorporated  into  an  extended  Kalman  filter  and  tested 
experimentally  in  [23]  using  a  tactical-grade  inertial  sensor. 
The  integrated  system  demonstrated  at  least  two  orders  of 
magnitude  improvement  over  the  inertial-only  navigation 
solution. 

In  this  paper,  the  method  of  stochastic  projections  [24]  is 
used  as  the  basis  for  tightly  integrating  a  low-cost  MEMS 
inertial  and  CMOS  imaging  sensor  using  an  Extended 
Kalman  Filter  (EKF).  In  the  following  section,  the  inte¬ 
gration  architecture  is  presented,  which  includes  the  under¬ 
lying  assumptions,  the  inertial  mechanization  algorithms, 
EKF  state  model,  measurement  model,  and  feature  track¬ 
ing  concept. 

DEVELOPMENT 

The  method  proposed  in  this  paper  employs  an  extended 
Kalman  filter  (EKF)  algorithm  [9,  10]  to  recursively  esti¬ 
mate  the  navigation  state  and  associated  errors  by  tracking 
the  pixel  locations  of  stationary  objects  in  an  image-aided 
inertial  system. 

Assumptions 

This  method  is  based  on  the  following  assumptions. 

•  A  strapdown  inertial  measurement  unit  (IMU)  is 
rigidly  attached  to  one  or  more  cameras.  Synchro¬ 
nized  raw  measurements  are  available  from  both  sen¬ 
sors. 

•  The  camera  images  areas  in  the  environment  which 
contain  some  stationary  objects. 

•  Binocular  measurements  are  available  which  provide 
an  indication  of  range  to  objects  in  the  environment. 

•  The  inertial  and  optical  sensors’  relative  position  and 
orientation  is  known  (see  [22]  for  a  discussion  of  bore- 
sight  calibration  procedures). 


Algorithm  Description 

The  system  parameters  (see  Table  1)  consist  of  the  nav¬ 
igation  parameters  (position,  velocity,  and  attitude),  iner¬ 
tial  sensor  biases,  and  a  vector  describing  the  location  of 
landmarks  of  interest  (y).  The  navigation  parameters  are 
calculated  using  body-frame  velocity  increment  (Avfc)  and 
angular  increment  (A 6bib)  measurements  from  the  inertial 
navigation  sensor  which  have  been  corrected  for  bias  er¬ 
rors  using  the  current  filter-computed  bias  estimates.  These 
measurements  are  integrated  from  an  initial  state  in  the  nav¬ 
igation  (local-level)  frame  using  mechanization  algorithms 
described  in  [21], 


Table  1:  System  Parameter  Definition 


Parameter 

Description 

pn 

Vehicle  position  in  navigation  frame 
(northing,  easting,  and  down) 

v" 

Vehicle  velocity  in  navigation  frame 
(north,  east,  down) 

b 

Vehicle  body  to  navigation  frame  DCM 

a  6 

Acclerometer  bias  vector 

bb 

Gyroscope  bias  vector 

c 

Location  of  landmark  m  in  the 
navigation  frame  (one  for  each  landmark 
currently  tracked) 

dh 

Camera-to-IMU  lever  arm  in  body  frame 

c* 

Camera-to-IMU  orientation  DCM 

An  Extended  Kalman  Filter  is  constructed  to  estimate  the 
errors  in  the  calculated  system  parameters.  In  order  to  min¬ 
imize  the  effects  of  linearization  errors,  the  system  param¬ 
eters  are  periodically  corrected  by  removing  the  current  er¬ 
ror  estimate  [9],  A  block  diagram  of  the  system  is  shown 
in  Figure  1 . 

The  position,  velocity,  and  attitude  errors  were  modeled 
as  a  stochastic  process  based  on  the  well-known  Pinson 
navigation  error  model  [21],  The  accelerometer  and  gyro¬ 
scopic  bias  errors  were  each  modeled  as  a  first-order  Gauss- 
Markov  process  [9],  based  on  the  specification  for  the  iner¬ 
tial  measurement  unit  (IMU).  The  landmarks  are  modeled 
as  stationary  with  respect  to  the  Earth.  A  small  amount 
of  process  noise  is  added  to  the  state  dynamics  to  promote 
filter  stability  [10]. 

Landmark  Track  Maintenance 

In  a  practical  system,  the  number  of  Kalman  Filter  states 
is  limited  by  available  computer  resources.  As  a  result,  the 
number  of  landmarks  actively  tracked  must  be  constrained. 
This  inherent  limitation  motivates  the  implementation  of  a 
track  maintenance  algorithm. 


Figure  1:  Image-aided  inertial  navigation  filter  block  di¬ 
agram.  In  this  filter,  the  location  of  stationary  objects  are 
tracked  and  used  to  estimate  and  update  the  errors  in  an 
inertial  navigation  system.  The  inertial  navigation  system 
is,  in  turn,  used  to  support  the  feature  tracking  loop. 


The  general  concept  for  the  track  maintenance  algorithm  is 
to  add  and  prune  landmark  tracks  in  order  to  provide  the 
“best”  navigation  information  to  the  filter.  Although  the 
optimal  landmark  choices  are  highly  dependent  on  the  tra¬ 
jectory  and  scene,  some  general  guidelines  were  used  in  the 
track  maintenance  algorithm. 

In  general,  features  which  can  be  easily  and  accurately 
tracked  for  long  periods  of  time  provide  the  best  naviga¬ 
tion  information.  This  implies  choosing  features  which 
are:  strong  and  easily  identified  (to  help  maintain  track),  lo¬ 
cally  distinct  (to  eliminate  false  correspondence),  and  well- 
separated  in  image  space  (to  maximize  filter  observability). 
Thus,  when  Kalman  Filter  landmark  track  states  are  avail¬ 
able,  the  feature  space  of  the  current  image  is  searched  and 
new  landmarks  are  added  based  on  the  above  criteria.  The 
filter  states  are  augmented  in  accordance  with  the  stochas¬ 
tic  projection  algorithm  defined  in  [24]. 

In  order  to  maintain  only  the  best  tracks,  stale  landmark 
tracks  (i.e.,  no  successful  correspondence  available  for  a 
given  period  of  time)  are  pruned  by  removing  the  associ¬ 
ated  filter  states.  Other  track  maintenance  approaches  are 
possible  which  could  theoretically  improve  the  track  per¬ 
formance  (e.g.,  semi-causal,  multiple  model,  or  velocity 
prediction),  however  these  approaches  will  not  be  pursued 
in  this  paper. 

Measurement  Model 

In  order  to  exploit  the  synergistic  properties  of  optical  and 
inertial  sensors,  the  navigation  and  feature  tracking  algo¬ 
rithms  are  tightly-coupled.  This  results  in  a  slight  mod¬ 
ification  to  the  standard  Kalman  filter  update  and  propa¬ 
gation  cycles  in  order  to  incorporate  the  feature  tracking 


loop.  The  tracking  loop  is  responsible  for:  incorporating 
new  landmark  tracks,  using  stochastic  projections  to  pre¬ 
dict  and  match  features  between  images,  providing  filter 
measurements,  and  deleting  stale  landmarks  from  the  filter. 

The  Kalman  filter  assists  the  tracking  algorithm  by  main¬ 
taining  and  propagating  the  minimum  mean-square  error 
state  estimate.  This  provides  the  stochastic  projections 
which  help  improve  the  speed  and  robustness  of  the  track¬ 
ing  loop. 

The  tracking  loop  incorporates  new  landmark  tracks  when 
necessary  by  determining  an  initial  estimate  of  the  land¬ 
mark  location  (using  either  a  terrain  model  or  binocu¬ 
lar  stereopsis  combined  with  the  current  navigation  state 
vector).  This  estimate,  along  with  the  calculated  covari¬ 
ance  and  cross-correlation  matrices  are  augmented  into  the 
Kalman  filter  state  vector  and  covariance  matrix.  Details 
on  the  mathematics  involved  in  the  calculation  of  the  above 
process  are  based  on  the  stochastic  projection  model  de¬ 
scribed  in  [24] . 

After  the  landmark  tracks  are  properly  augmented  into  the 
Kalman  filter,  the  standard  propagation  algorithms  are  used 
to  predict  the  augmented  state  to  the  time  of  the  next  image. 
The  location  of  each  landmark  (along  with  arbitrary  uncer¬ 
tainty  ellipsoid)  can  then  be  projected  into  the  feature  space 
of  the  new  image.  In  this  paper,  the  feature  space  corre¬ 
sponds  to  a  two-dimensional  pixel  location  and  associated 
uncertainty  ellipse.  The  tracking  algorithm  then  searches 
this  uncertainty  ellipse  for  a  feature  which  has  similar  char¬ 
acteristics  to  the  reference  feature  and  is  distinct.  In  this 
paper,  a  2  —  a  ellipse  was  used.  An  example  of  feature  pre¬ 
diction  is  shown  in  Figure  2.  The  reader  is  referred  to  [24] 
for  more  details  regarding  the  feature  prediction  algorithm. 


Figure  2:  Stochastic  feature  projection.  Optical  features 
of  interest  are  projected  into  future  images  using  inertial 
measurements  and  stochastic  projections. 


Once  the  landmark  tracker  has  determined  a  corresponding 
match,  the  pixel  location  of  the  feature,  corrected  for  opti¬ 
cal  distortion,  is  used  to  update  the  navigation  state.  The 
associated  measurement  equations  are  described  in  further 
detail  in  [23]. 

Measurement  Accuracy  Considerations 

The  inertial-aided  feature  correspondence  algorithm  pre¬ 
sented  in  the  previous  section  leverages  inertial  measure¬ 
ments  to  statistically  constrain  the  feature  correspondence 
search  between  images.  As  shown  in  [24],  the  correspon¬ 
dence  search  region  is  the  projection  of  a  random  vector 
into  the  feature  space.  The  statistics  of  this  random  vector 
are  influenced  by  the  inertial  measurement  errors,  the  time 
between  images,  the  motion  trajectory,  and  the  scene  itself. 
Examining  this  relationship  allows  the  designer  to  predict, 
and  potentially  compensate  for,  the  effects  of  lower  quality 
inertial  sensors  by  varying  the  image  sampling  rate. 

As  a  result  of  this  relationship,  the  image  sample  rate  is 
increased  by  2.5  times  in  order  to  compensate  for  the  lower 
performance  level  of  the  consumer-grade  IMU  compared  to 
the  tactical-grade  sensor.  This  resulted  in  a  2.5  Hz  image 
collection  rate,  which  was  the  upper  limit  of  the  recording 
hardware. 

SYSTEM  TESTS 

The  imaging  and  inertial  fusion  navigation  algorithm  is 
evaluated  using  both  simulated  and  experimental  ground 
profiles.  The  profiles  are  designed  to  provide  a  range  of 
image  types  in  order  to  exercise  the  feature  tracking  algo¬ 
rithm.  The  simulation  and  results  are  presented  in  the  next 
section. 

The  data  collection  system  consisted  of  a  consumer-grade 
MEMS  IMU  and  two  digital  cameras  (see  Figure  3).  The 
IMU  was  a  Crista  consumer-grade  MEMS  unit  which  mea¬ 
sured  acceleration  and  angular  rate  at  100  Hz.  The  digital 
cameras  were  both  Pixelink  A-741  machine  vision  cameras 
which  incorporated  a  global  shutter  feature  and  a  Firewire 
interface.  The  lenses  were  wide-angle  Pentax  lenses  with 
approximately  90  degrees  field  of  view.  The  sensors  were 
mounted  on  an  aluminum  plate  and  calibrated  using  pro¬ 
cedures  similar  to  those  described  in  [22].  Images  were 
captured  at  approximately  2.5  Hz.  In  addition,  a  Honey¬ 
well  HG1700,  tactical-grade  inertial  measurement  unit  was 
co-mounted  on  the  platform  in  order  to  provide  a  one-to- 
one  performance  comparison  between  different  grades  of 
inertial  sensors. 

Simulation 

The  algorithm  was  tested  using  a  Monte  Carlo  simulation 


Figure  3:  Data  collection  system.  The  data  collection 
system  consisted  of  a  consumer- grade  MEMS  IMU  and 
monochrome  digital  cameras.  A  tactical- grade  IMU  was 
co-mounted  on  the  platform  in  order  to  provide  a  perfor¬ 
mance  comparison  between  different  grades  of  inertial  sen¬ 
sors. 


of  a  standard  indoor  profile.  The  profile  consisted  of  a 
straight  corridor,  designed  to  be  similar  to  the  indoor  ex¬ 
perimental  data  collection. 

An  accurate  simulation  of  the  navigation  environment  re¬ 
quires  simulating  the  performance  of  the  sensors  in  re¬ 
sponse  to  a  true  motion  trajectory.  The  trajectory  was  gen¬ 
erated  using  ProfGen  version  8.19  software  package  [11]. 
For  each  Monte  Carlo  navigation  simulation  run,  the  iner¬ 
tial  sensor  measurements  are  generated  using  the  true  tra¬ 
jectory  and  an  inertial  sensor  error  model. 

Because  of  the  inherent  complexity  of  the  optical  environ¬ 
ment,  it  is  beyond  the  scope  of  this  paper  to  generate  sim¬ 
ulated  images.  Instead,  a  simulated  feature  set  was  cre¬ 
ated  by  randomly  distributing  features  along  a  corridor  sur¬ 
rounding  the  true  trajectory.  The  features  are  each  given 
random  descriptor  vectors  in  order  to  exercise  the  feature 
tracking  algorithm.  While  this  optical  simulation  method 
is  appropriate  for  testing  the  image  and  inertial  fusion  al¬ 
gorithm,  the  results  are  not  directly  comparable  to  the  real 
system  performance,  because  imaging  issues  such  as  light¬ 
ing  conditions,  motion  blur,  and  affine  changes  in  the  fea¬ 
ture  descriptor  due  to  pose  changes  are  not  modeled. 


alignment,  the  sensor  platform  accelerated  to  0.5  meters  per 
second,  maintained  this  velocity  until  the  end  of  the  corri¬ 
dor,  then  accelerated  to  a  stop  at  the  end.  The  platform 
remained  stationary  for  60  seconds  after  coming  to  a  stop. 
This  resulted  in  a  660-second  image  and  inertial  navigation 
profile.  Simulated  images  are  collected  at  2  Hz. 

A  Monte  Carlo  simulation  was  conducted  using  inertial 
sensor  models  representing  the  H1700  tactical-grade  IMU 
and  the  Crista  consumer-grade  IMU.  Each  simulation  con¬ 
sisted  of  60  runs,  each  with  randomly  generated  inertial 
measurement  errors  due  to  random  walks,  sensor  bias,  and 
sensor  scale -factor  errors.  In  order  to  mitigate  any  poten¬ 
tial  effects  due  to  the  location  of  the  features  in  the  sim¬ 
ulated  environment,  the  feature  locations  and  descriptors 
were  randomly  generated  every  20  runs. 

The  position  errors  using  the  tactical-grade  IMU  are  shown 
in  Figures  4.  As  expected,  the  inertial  and  imaging  mea¬ 
surement  errors  accumulate,  resulting  in  position  drift. 
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Figure  4:  Simulated  60-run  Monte  Carlo  position  error  re¬ 
sults  for  indoor  profile  with  a  tactical- grade  inertial  sensor 
and  image  aiding  using  landmarks  of  opportunity.  The  po¬ 
sition  error  sample  functions  are  indicated  by  blue  dotted 
lines.  The  ensemble  mean  and  standard  deviation  are  indi¬ 
cated  by  the  green  and  red  solid  lines,  respectively. 

The  position  errors  using  the  consumer-grade  IMU  are 
shown  in  Figures  5.  In  addition  to  proportionally  larger  er¬ 
rors  in  position,  significant  excursions  in  position  are  noted, 
which  are  the  effects  of  increased  attitude  errors. 


The  simulated  corridor  was  3  meters  wide,  3  meters  high, 
and  approximately  300  meters  long.  Features  were  ran¬ 
domly  generated  on  the  walls,  floor  and  ceiling  of  the  cor¬ 
ridor  with  an  average  spacing  of  0.25  features  per  square 
meter.  Each  feature  was  given  a  random  primary  length  and 
orientation,  which,  combined  with  the  true  pose  of  the  sen¬ 
sor,  resulted  in  accurately  simulated  scale  and  orientation 
parameters  in  feature  space.  After  a  60-second  stationary 


Root-sum-squared  (RSS)  errors  are  analyzed  in  order  to 
provide  a  one-dimensional  metric  for  a  more  direct  com¬ 
parison  of  the  simulated  system  performance  for  different 
profiles.  The  RSS  errors  comparing  the  free -inertial  and 
image-aided  performance  is  shown  in  Figures  6-8.  Over  the 
10-minute  indoor  profile,  incorporating  the  image-aiding 
measurements  improves  the  errors  for  both  the  consumer 
and  tactical-grade  sensors  by  many  orders  of  magnitude. 
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Figure  5:  Simulated  60-run  Monte  Carlo  position  error 
results  for  indoor  profile  with  a  consumer-grade  inertial 
sensor  and  image  aiding  using  landmarks  of  opportunity. 
The  position  error  sample  functions  are  indicated  by  blue 
dotted  lines.  The  ensemble  mean  and  standard  deviation 
are  indicated  by  the  green  and  red  solid  lines,  respectively. 


Figure  6:  Simulated  60-run  Monte  Carlo  root-sum- 
squared  (RSS)  horizontal  position  error  for  indoor  pro¬ 
file  using  both  consumer-grade  and  tactical-grade  iner¬ 
tial  sensors.  The  results  are  shown  for  four  cases:  1 ) 
consumer-grade  free  inertial,  2)  consumer-grade  opportu¬ 
nity  landmark  tracking,  3)  tactical-grade  free  inertial,  and 
4)  tactical-grade  opportunity  landmark  tracking. 


In  addition,  the  image-aided  consumer-grade  sensor  nearly 
equals  the  position  error  performance  of  the  image-aided 
tactical-grade  sensor,  until  the  consumer-grade  sensor  be¬ 
gins  to  show  some  attitude  divergences  after  approximately 
400  seconds. 

In  the  next  section,  the  experimental  data  collection  profiles 
and  results  are  presented. 

Experiment 

The  algorithm  is  tested  experimentally  using  two  ground 
navigation  profiles  designed  to  examine  the  operation  of 
the  feature  tracking  system  in  a  real-world  environment. 
The  profile  consisted  of  a  closed  path  in  an  indoor  environ¬ 
ment.  The  path  began  and  ended  at  the  same  location  and 
orientation  in  the  Advanced  Navigation  Technology  (ANT) 
Center  laboratory,  at  the  Air  Force  Institute  of  Technology. 
As  in  the  previous  profile,  the  data  collection  began  with  a 
10-minute  stationary  alignment  period.  After  the  alignment 
period,  the  sensor  was  moved  in  a  10-minute  loop  around 
the  hallways  of  the  building.  In  contrast  to  the  previous 
profile,  the  sensor  was  pointed  primarily  in  the  direction  of 
travel.  No  prior  knowledge  was  provided  to  the  algorithm 
regarding  the  location  of  features  or  structure  of  the  envi¬ 
ronment.  A  sample  image  from  the  indoor  profile  is  shown 
in  Figure  9. 

The  indoor  profile  presents  the  algorithm  with  different 
challenges  from  a  feature  tracking  perspective.  The  indoor 
environment  consists  of  repetitive,  visually  identical  fea¬ 
tures  (e.g.,  floor  tiles,  lights,  etc.),  which  can  easily  cause 


confusion  for  the  feature  tracking  algorithm.  In  addition, 
reflections  from  windows  and  other  shiny  surfaces  might 
not  be  interpreted  properly  by  the  filter  and  could  poten¬ 
tially  result  in  navigation  errors.  Finally,  the  lower  light 
intensity  levels  and  large  areas  with  poor  contrast  (e.g., 
smooth,  featureless  walls)  presents  a  relatively  stark  fea¬ 
ture  space.  The  indoor  profile  is  performed  twice  for  both 
the  tactical  and  navigation-grade  sensors. 

The  filters’  estimates  of  the  trajectories  are  overlayed  on 
a  floor  plan  of  the  building  in  Figures  10  and  11  for  the 
tactical  and  consumer-grade  inertial  sensors,  respectively. 
In  each  figure,  a  comparison  is  made  between  the  fused 
image-aided  inertial  trajectory  estimate,  the  image-aided 
inertial  trajectory  with  stochastic  constraints  disabled,  and 
a  free  inertial  trajectory.  For  both  tactical  and  consumer- 
grade  sensors,  the  estimated  trajectory  generally  corre¬ 
sponds  to  the  building’s  hallways,  with  excursions  of  less 
than  3  meters.  In  addition,  the  results  of  the  free-inertial 
trajectories  show  the  inherent  lack  of  accuracy  of  the  in¬ 
ertial  sensor.  With  stochastic  constraints  purposely  dis¬ 
abled,  the  trajectory  estimates  show  relatively  large  trajec¬ 
tory  errors  due  to  false  correspondence  matches.  This  il¬ 
lustrates  the  catastrophic  effects  of  incorporating  false  up¬ 
dates  into  an  Extended  Kalman  Filter  with  inertial  feedback 
and  demonstrates  the  inherent  strength  of  applying  robust 
correspondence  methods,  and  in  particular  stochastic  con¬ 
straints. 

The  filter’s  estimated  trajectory  for  the  image-aided 
consumer-grade  sensor  (run  two)  is  examined  in  more  de¬ 
tail  in  Figure  14,  where  the  estimated  location  of  landmarks 
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Figure  7:  Simulated  60-run  Monte  Carlo  root-sum- 
squared  (RSS)  velocity  error  for  indoor  profile  using  both 
consumer- grade  and  tactical-grade  inertial  sensors.  The 
results  are  shown  for  four  cases:  1)  consumer-grade  free 
inertial,  2)  consumer-grade  opportunity  landmark  track¬ 
ing,  3)  tactical-grade  free  inertial,  and  4)  tactical- grade 
opportunity  landmark  tracking. 


used  for  tracking  are  highlighted.  Note  the  landmarks  cor¬ 
respond  to  the  building  walls,  ceilings,  and  floors.  More  de¬ 
tail  of  the  start/stop  area  is  shown  in  Figure  13.  A  compar¬ 
ison  of  all  image-aided  inertial  navigation  results  for  both 
the  tactical  and  consumer-grade  sensors  is  shown  in  Fig¬ 
ure  12.  The  difference  in  the  estimated  start  and  stop  lo¬ 
cations  shows  the  accumulated  errors  in  the  filter  over  the 
path.  Over  the  10-minute  profile,  the  path  closure  errors 
are  less  than  5  m  in  the  horizontal  plane  and  less  than  5  in 
in  the  vertical  for  all  sensors.  Again,  this  is  a  significant 
improvement  over  the  free -inertial  performance. 

In  each  case,  incorporation  of  image  updates  into  the  navi¬ 
gation  filter  improves  the  position  error  by  several  orders 
of  magnitude  over  the  respective  inertial-only  solutions. 
In  addition,  the  integrated  consumer-grade  inertial  solu¬ 
tion  demonstrates  nearly  equivalent  performance  to  the  in¬ 
tegrated  tactical-grade  inertial  solution. 


■  Consumer-grade  Free  Inertial 
Consumer-grade  Image  Aided 

■  Tactical-grade  Free  Inertial 
Tactical-grade  Image  Aided 
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Figure  8:  Simulated  60-run  Monte  Carlo  root-sum- 
squared  (RSS)  attitude  error  for  indoor  profile  using  both 
consumer-grade  and  tactical- grade  inertial  sensors.  The 
results  are  shown  for  four  cases:  1)  consumer-grade  free 
inertial,  2)  consumer-grade  opportunity  landmark  track¬ 
ing,  3)  tactical- grade  free  inertial,  and  4)  tactical- grade 
opportunity  landmark  tracking. 


Figure  9:  Sample  image  from  indoor  data  collection.  The 
indoor  data  collection  presents  the  filter  with  man-made 
features  in  an  office  environment.  The  crosses  and  ellipses 
indicate  the  locations  and  uncertainty  of  currently  tracked 
landmarks. 


Figure  10:  Estimated  path  from  indoor  data  collection  ( run 
two)  using  tactical-grade  inertial  sensor.  The  filter’s  esti¬ 
mate  of  the  path  (indicated  by  the  solid  line)  agrees  well 
with  the  blown  path  (indicated  by  the  dotted  line).  The 
inertial-only  best  estimate  of  trajectory  (indicated  by  the 
dash-dotted  line)  and  image-aided  inertial  with  stochastic 
constraints  disabled  (indicated  by  the  dashed  line)  show 
large  errors  in  position  and  heading.  The  inertial-only  so¬ 
lution  exceeds  the  scale  of  the  image  after  156  seconds. 


Figure  11:  Estimated  path  from  indoor  data  collection  ( run 
two)  using  consumer-grade  inertial  sensor.  The  filter’s  es¬ 
timate  of  the  path  (indicated  by  the  solid  line)  agrees  well 
with  the  known  path  (indicated  by  the  dotted  line).  The 
inertial-only  best  estimate  of  trajectory  (indicated  by  the 
dash-dotted  line)  and  image-aided  inertial  with  stochastic 
constraints  disabled  (indicated  by  the  dashed  line)  show 
large  errors  in  position  and  heading.  The  inertial-only  so¬ 
lution  exceeds  the  scale  of  the  image  after  11  seconds. 


Figure  12:  Performance  comparison  for  image-aided  tac¬ 
tical  and  consumer-grade  inertial  sensors  for  indoor  profile 
runs  one  and  two.  The  common  start  and  stop  location  for 
both  runs  is  indicated  by  the  “o”  symbol.  The  estimated 
stop  location  for  each  run  is  indicated  by  an  “x”  symbol. 


Figure  13:  Enhanced  detail  of  the  start/stop  area  illustrat¬ 
ing  the  estimated  trajectory  and  feature  locations  for  both 
the  image-aided  tactical  and  consumer-grade  inertial  sen¬ 
sors  for  indoor  profile  runs  one  and  two.  The  difference  be¬ 
tween  the  estimated  start  and  stop  location  illustrates  the 
accumulated  position  error. 


a)  Entire  path  estimate 


b)  First  half  of  path  with  tracked  feature  locations 


c)  Second  half  of  path  with  tracked  feature  locations 


d)  Start/stop  area  detail 


Figure  14:  Estimated  path  and  feature  locations  from  indoor  data  collection  (run  two)  for  consumer- grade  inertial  sensor. 
Pane  (a)  shows  the  entire  path  estimate.  Pane  (b)  shows  the  first  half  of  the  path  along  with  the  estimated  location  of  the 
features  (indicated  by  “x”  symbols).  Pane  (c)  shows  the  last  half  of  the  path  and  estimated  feature  locations.  Pane  (d)  provides 
detail  of  the  start/stop  area. 


CONCLUSIONS 

In  this  paper,  an  algorithm  is  presented  which  integrates 
low-cost  inertial  and  image  sensors  to  provide  an  enhanced 
navigation  solution.  The  integration  is  accomplished  using 
a  fusion  technique  designed  to  tightly-couple  the  respective 
sensors  using  a  statistically-rigorous  feature  transformation 
algorithm. 

The  integrated  system  was  tested  using  a  combination  of 
Monte  Carlo  simulation  and  experimental  data  collections. 
The  simulation  predicted  a  significant  improvement  by  fus¬ 
ing  image  and  inertial  measurements  for  both  the  consumer 
and  tactical-grade  inertial  sensors.  In  addition,  the  sim¬ 
ulation  revealed  small  differences  in  attitude-error  stabil¬ 
ity,  which  negatively  influenced  the  long-term  stability  of 
the  integrated  consumer-grade  sensor  over  the  integrated 
tactical-grade  sensor. 

The  experiment  demonstrated  multiple  orders-of- 
magnitude  improvement  between  image-aided  and 
unaided  inertial  position  solutions.  Over  the  10-minute, 
unconstrained,  indoor-profile,  both  integrated  sensors  ex¬ 
hibited  closed-path  navigation  errors  of  less  than  5  meters, 
which  is  a  significant  improvement  over  the  respective 
unaided  systems. 

In  summary,  this  research  develops  a  series  of  techniques 
to  provide  autonomous,  passive  navigation  by  incorporat¬ 
ing  image  and  inertial  measurements.  The  method  demon¬ 
strates  navigation-quality  performance  using  only  low-cost 
sensors  and  passive  updates. 

DISCLAIMER 

The  views  expressed  in  this  article  are  those  of  the  au¬ 
thor  and  do  not  reflect  the  official  policy  or  position  of  the 
United  States  Air  Force,  Department  of  Defense,  or  the  U.S 
Government. 
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